#!/usr/bin/python
# encoding: utf-8

import os
import json
import time
import traceback
import threading
import jkzuc
import configparser
import shutil
import collections
import zlib
import subprocess
import multiprocessing
import stat
import tool_function
import pylog
from enum import Enum


JOGCON_WARNING = False
JOG_FLAG = False
CURRENT_TIME = 0

ROBOT_MAX_JOINT = 7

MASS = 0
CENTROID = (0, 0, 0)
IDENTIFY_COMPLETE = 1

maps_data = (
    "joint_actual_position",
    "actual_position",
    "din",
    "dout",
    "ain",
    "aout",
    "tio_din",
    "tio_dout",
    "tio_ain",
    "mb_slave_din",
    "mb_slave_dout",
    "mb_slave_ain",
    "mb_slave_aout",
    "pn_dev_din",
    "pn_dev_dout",
    "pn_dev_ain",
    "pn_dev_aout",
    "eip_adpt_din",
    "eip_adpt_dout",
    "eip_adpt_ain",
    "eip_adpt_aout",
    "task_state",
    "homed",
    "task_mode",
    "interp_state",
    "enabled",
    "paused",
    "rapidrate",
    "current_tool_id",
    "current_user_id",
    "protective_stop",
    "on_soft_limit",
    "emergency_stop",
    "drag_near_limit",
    "powered_on",
    "inpos",
    "executing_line",
    "curr_tcp_trans_vel",
)

# for optionalInfoConfig.ini
section_attr = "ATTROPTIONALDATA"
section_stat = "STATOPTIONALDATA"
section_port = "PORTCONFIG"
maps_attr_used = []
maps_stat_used = []
port10000_delay_ms = 1000


delmitator = r":"
changeline = "\r\n"

c = jkzuc.command()
s = jkzuc.stat()

tcp_logger = pylog.tcp_logger


class ErrCodeTcp(Enum):
    SUCCESS = (0, "")
    UNSUPPORTED_FUNCTION = (1, "unsupported command")
    FAILED = (2, "function called failed")
    PERMISSION_DENY = (3, "permission deny, please check control source")
    NO_POWER_OFF = (4, "please power off the robot first")
    NO_POWER_ON = (5, "please power on the robot first")
    NO_ENABLE_OFF = (6, "please disable the robot first")
    NO_ENABLE_ON = (7, "please enable the robot first")
    EMERGENCY_STOP = (8, "emergency stop active")
    PROTECTIVE_STOP = (9, "protective stop active")
    PARAM_OUT_OF_RANGE = (10, "one of the paramater out of range")
    VAR_ALIAS_EXIST = (11, "alias name must be unique")
    OPERATIONS_PROHIBITED_WHEN_PROGRAM_NO_IDLE = (
        12,
        "cannot do that when program running or pause",
    )
    PARAM_ERROR = (13, "one of input param is not suitable")

    def __init__(self, value, label):
        self._value_ = value
        self.label = label
        self.str_value = str(self._value_)


class TcpCommandException(Exception):

    def __init__(self, errCode, errMsg):
        self.ret_dict = {
            "errorMsg": str.format("Exception: {}", errMsg),
            "errorCode": errCode,
        }

    @property
    def RetDict(self):
        return self.ret_dict


class UnknownException(TcpCommandException):

    def __init__(self):
        TcpCommandException.__init__(self, "1", "function call failed")


class InvalidParamException(TcpCommandException):

    def __init__(self):
        TcpCommandException.__init__(self, "2", "invalid param")


class InvalidCmdNameException(TcpCommandException):

    def __init__(self):
        TcpCommandException.__init__(self, "3", "invalid command name")


class ParamChecker(object):

    def __init__(self):
        self.__next = None

    def Check(self, val):
        if not self.CheckImpl(val):
            return False
        elif self.__next != None:
            return self.__next.Check(val)
        else:
            return True

    def CheckImpl(self, val):
        return True

    def AddNext(self, checker):
        self.__next = checker
        return self.__next


class NumCheckerCreator:

    class GEZero(ParamChecker):
        """greater or equal than 0"""

        def CheckImpl(self, val):
            return val >= 0

    class IsNum(ParamChecker):

        def CheckImpl(self, val):
            return isinstance(val, (int, long, float))

    @staticmethod
    def GEZeroChecker():
        checker = NumCheckerCreator.IsNum()
        checker.AddNext(NumCheckerCreator.GEZero())
        return checker


class Validator(object):

    def __init__(self, checker):
        self.checker = checker
        pass

    def Validate(self, val, exceptType=InvalidParamException):
        if not self.checker.Check(val):
            raise exceptType()


# this class is defined to keep keys case sensitive
class MyConfigParser(configparser.ConfigParser):

    def __init__(self, defaults=None):
        configparser.ConfigParser.__init__(self, defaults=defaults)

    def optionxform(self, optionstr):
        return optionstr


# 确认模式正确
def ensure_mode(m, *p):
    s.poll()
    if s.task_mode == m or s.task_mode in p:
        return True
    c.mode(m)  # task_mode
    c.wait_complete()
    s.poll()
    return True


def timer_shutdown(self):
    tcp_logger().info("[gRPC] timer shutdown")
    try:
        tool_function_ctx = tool_function.ModuleCtx(tcp_logger)
        tool_function.runCmdAsRoot("shutdown", tool_function_ctx)
    except Exception:
        tcp_logger().warn("[gRPC] error shutdown")


############################### TCP CMD PROCESS ############################
def enable_robot(**cmdDict):
    ret_dict = {}
    c.rob_enable()
    time.sleep(3)
    s.poll()
    for jnum in range(0, s.robot_joint_num):
        if not (s.homed[jnum]):
            c.teleop_enable(0)
            c.wait_complete()
            c.home(-1)
            c.wait_complete()
            break
    t1 = time.time()
    while True:
        s.poll()
        if s.enabled:
            ret_dict["enabled status"] = "True"
            tcp_logger().info("[SDK] robot enable succeeded")
            return ret_dict
        t2 = time.time()
        time.sleep(1)
        if t2 - t1 > 30:
            break
    ret_dict["errorCode"] = "2"
    ret_dict["enable_time"] = str(t2 - t1) + " s"
    ret_dict["errorMsg"] = "enable robot failed"
    tcp_logger().warn("[SDK] robot enable failed")
    return ret_dict


def disable_robot(**cmdDict):
    c.rob_disable()
    tcp_logger().info("[SDK] robot disable")
    ret_dict = {}
    return ret_dict


def load_program(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        if s.interp_state != jkzuc.INTERP_IDLE:
            ret_dict["errorCode"] = "2"
            ret_dict["errorMsg"] = "Cannot load program when a program is running"
            return ret_dict
        c.teleop_enable(0)
        c.wait_complete()
        c.task_plan_synch()
        c.wait_complete()
        dir_path = cmdDict["programName"][0:-4]
        filePath = jkzuc.jkroot_dir + "/scripts/"
        if str(cmdDict["programName"]).find("track") == -1:
            filePath = filePath + "program/" + dir_path + "/" + cmdDict["programName"]
        else:
            folder_path, file_name = os.path.split(cmdDict["programName"])
            name, extension = os.path.splitext(file_name)
            filePath = filePath + folder_path + "/" + name + "/" + file_name
        tcp_logger().info("[SDK] open program:{}".format(filePath))
        c.program_open(filePath)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict["errorCode"] = "2"
        ret_dict["errorMsg"] = "call failed"
        return ret_dict


def play_program(**cmdDict):
    c.teleop_enable(0)
    c.wait_complete()
    s.poll()
    for jnum in range(0, s.robot_joint_num):
        if not (s.homed[jnum]):
            c.home(-1)
            c.wait_complete()
            break
    s.poll()
    if s.task_mode != jkzuc.MODE_AUTO:
        c.mode(jkzuc.MODE_AUTO)  # task_mode
        c.wait_complete()
        s.poll()
    tcp_logger().info("[SDK] run program")
    c.auto(jkzuc.AUTO_RUN, 0)
    ret_dict = {}
    return ret_dict


def get_loaded_program(**cmdDict):
    s.poll()
    program_name = os.path.basename(s.file)

    ret_dict = {}
    ret_dict["programName"] = program_name
    return ret_dict


def pause_program(**cmdDict):
    s.poll()
    ret_dict = {}
    if s.task_mode != jkzuc.MODE_AUTO:
        ret_dict["errorCode"] = "3"
        ret_dict["errorMsg"] = "mode is not MODE_AUTO"
        return ret_dict

    ensure_mode(jkzuc.MODE_AUTO)
    c.auto(jkzuc.AUTO_PAUSE)
    tcp_logger().info("[SDK] pause program")
    return ret_dict


def resume_program(**cmdDict):
    s.poll()
    ret_dict = {}
    if not s.traj_paused:
        return ret_dict
    if s.task_mode not in (jkzuc.MODE_AUTO, jkzuc.MODE_MDI):
        return ret_dict
    ensure_mode(jkzuc.MODE_AUTO, jkzuc.MODE_MDI)
    c.auto(jkzuc.AUTO_RESUME)
    tcp_logger().info("[SDK] resume program")
    return ret_dict


def stop_program(**cmdDict):
    s.poll()
    if s.enabled:
        c.abort()
        c.teleop_enable(0)
        c.wait_complete()
        tcp_logger().info("[SDK] stop program")
    ret_dict = {}
    return ret_dict


def get_program_state(**cmdDict):
    s.poll()

    ret_dict = {}
    program_state = s.interp_state
    if program_state == 0:
        ret_dict["programState"] = "idle"
    elif program_state == 1 or program_state == 3:
        ret_dict["programState"] = "running"
    elif program_state == 2:
        ret_dict["programState"] = "paused"

    return ret_dict


def power_on(**cmdDict):
    ret_dict = {}
    c.scb_power_on()
    tcp_logger().info("[SDK] robot power on")
    # time.sleep(8)
    t1 = time.time()
    while True:
        s.poll()
        if s.powered_on:
            ret_dict["power status"] = "True"
            tcp_logger().info("[SDK] robot power on succeeded")
            return ret_dict
        t2 = time.time()
        time.sleep(1)
        if t2 - t1 > 60:
            break
    ret_dict["errorCode"] = "2"
    ret_dict["errorMsg"] = "power on failed"
    return ret_dict


def power_off(**cmdDict):
    c.scb_power_off()
    tcp_logger().info("[SDK] robot power off")
    ret_dict = {}
    return ret_dict


def joint_move(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        robid = 0
        if "robid" in cmdDict:
            robid = cmdDict["robid"]
        joint_pos = cmdDict["jointPosition"]
        speed = cmdDict["speed"]
        relFlag = int(cmdDict["relFlag"])
        accel = 90
        tol = 0
        end_cond = [-1, -1, -1]
        executing_line_id = 0
        if cmdDict.has_key("accel"):
            accel = cmdDict["accel"]
        if cmdDict.has_key("tol"):
            tol = cmdDict["tol"]
        if cmdDict.has_key("executing_line_id"):
            executing_line_id = cmdDict["executing_line_id"]
        if cmdDict.has_key("end_cond"):
            end_cond = cmdDict["end_cond"]
            di_type = end_cond["di_type"]
            di_index = end_cond["di_index"]
            di_state = end_cond["di_state"]
            end_cond = [di_type, di_index, di_state]

        while len(joint_pos) < ROBOT_MAX_JOINT:
            joint_pos.append(0)

        ensure_mode(jkzuc.MODE_AUTO)
        c.jog_movj(
            relFlag,
            joint_pos[0],
            joint_pos[1],
            joint_pos[2],
            joint_pos[3],
            joint_pos[4],
            joint_pos[5],
            joint_pos[6],
            speed,
            accel,
            tol,
            executing_line_id,
            end_cond,
            robid
        )
        tcp_logger().info(
            "[SDK] joint move: {}, {}, {}, {}, {}, {}".format(
                joint_pos, speed, accel, tol, executing_line_id, end_cond
            )
        )

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict["errorCode"] = "2"
        ret_dict["errorMsg"] = "call joint_move failed"
        return ret_dict


def end_move(**cmdDict):
    ret_dict = {}
    try:
        joint_data = get_data_joint_position()
        endPosition = cmdDict["endPosition"]
        speed = cmdDict["speed"]
        accel = 90
        tol = 0
        end_cond = [-1, -1, -1]
        executing_line_id = 0
        if cmdDict.has_key("accel"):
            accel = cmdDict["accel"]
        if cmdDict.has_key("tol"):
            tol = cmdDict["tol"]
        if cmdDict.has_key("executing_line_id"):
            executing_line_id = cmdDict["executing_line_id"]
        if cmdDict.has_key("end_cond"):
            end_cond = cmdDict["end_cond"]
            di_type = end_cond["di_type"]
            di_index = end_cond["di_index"]
            di_state = end_cond["di_state"]
            end_cond = [di_type, di_index, di_state]

        while len(endPosition) < ROBOT_MAX_JOINT:
            endPosition.append(0)

        ensure_mode(jkzuc.MODE_AUTO)
        c.jog_movjp(
            0,
            endPosition[0],
            endPosition[1],
            endPosition[2],
            endPosition[3],
            endPosition[4],
            endPosition[5],
            endPosition[6],
            speed,
            accel,
            tol,
            executing_line_id,
            end_cond,
        )
        tcp_logger().info(
            "[SDK] end move: {}, {}, {}, {}, {}, {}".format(
                endPosition, speed, accel, tol, executing_line_id, end_cond
            )
        )

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict["errorCode"] = "2"
        ret_dict["errorMsg"] = "call end_move failed"
        return ret_dict


def moveL(**cmdDict):
    ret_dict = {}
    try:
        cart_pos = cmdDict["cartPosition"]
        speed = cmdDict["speed"]
        relFlag = int(cmdDict["relFlag"])
        accel = 500.0
        tol = 0
        ori_vel = 180.0
        ori_acc = 720.0
        end_cond = [-1, -1, -1]
        executing_line_id = 0
        if cmdDict.has_key("accel"):
            accel = cmdDict["accel"]
        if cmdDict.has_key("tol"):
            tol = cmdDict["tol"]
        if cmdDict.has_key("executing_line_id"):
            executing_line_id = cmdDict["executing_line_id"]
        if cmdDict.has_key("end_cond"):
            end_cond = cmdDict["end_cond"]
            di_type = end_cond["di_type"]
            di_index = end_cond["di_index"]
            di_state = end_cond["di_state"]
            end_cond = [di_type, di_index, di_state]
        if cmdDict.has_key("ori_vel"):
            ori_vel = cmdDict["ori_vel"]
        if cmdDict.has_key("ori_acc"):
            ori_acc = cmdDict["ori_acc"]
        s.poll()
        ensure_mode(jkzuc.MODE_AUTO)
        c.jog_movl(
            relFlag,
            cart_pos[0],
            cart_pos[1],
            cart_pos[2],
            cart_pos[3],
            cart_pos[4],
            cart_pos[5],
            speed,
            accel,
            tol,
            executing_line_id,
            end_cond,
            ori_vel,
            ori_acc,
        )
        tcp_logger().info(
            "[SDK] linear move: {}, {}, {}, {}, {}, {}, {}, {}".format(
                cart_pos,
                speed,
                accel,
                tol,
                executing_line_id,
                end_cond,
                ori_vel,
                ori_acc,
            )
        )

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict["errorCode"] = "2"
        ret_dict["errorMsg"] = "call moveL failed"
        return ret_dict


def movc(**cmdDict):
    ret_dict = {}
    try:
        tcp_logger().warn("movc")
        relFlag = int(cmdDict["relFlag"])
        relFlag = cmdDict["relFlag"]
        pos_mid = cmdDict["pos_mid"]
        pos_end = cmdDict["pos_end"]
        speed = cmdDict["speed"]
        accel = cmdDict["accel"]
        tol = cmdDict["tol"]
        executing_line_id = 0
        circle_cnt = 0
        end_cond = [-1, -1, -1]
        circle_mode = 0
        if cmdDict.has_key("circle_cnt"):
            circle_cnt = cmdDict["circle_cnt"]
        if cmdDict.has_key("executing_line_id"):
            executing_line_id = cmdDict["executing_line_id"]
        if cmdDict.has_key("end_cond"):
            end_cond = cmdDict["end_cond"]
            di_type = end_cond["di_type"]
            di_index = end_cond["di_index"]
            di_state = end_cond["di_state"]
            end_cond = [di_type, di_index, di_state]
        if cmdDict.has_key("circle_mode"):
            circle_mode = cmdDict["circle_mode"]
        ensure_mode(jkzuc.MODE_AUTO)

        c.jog_movc(
            relFlag,
            pos_mid,
            pos_end,
            speed,
            accel,
            tol,
            executing_line_id,
            end_cond,
            circle_cnt,
            circle_mode,
        )
        tcp_logger().info(
            "[SDK] circle move: {}, {}, {}, {}, {}, {}, {}, {}, {}".format(
                pos_mid,
                pos_end,
                speed,
                accel,
                tol,
                circle_cnt,
                executing_line_id,
                end_cond,
                circle_mode,
            )
        )
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict["errorCode"] = "2"
        ret_dict["errorMsg"] = "call movc failed"
        return ret_dict


def kine_inverse(**cmdDict):
    ret_dict = {}
    cart_pos = cmdDict["cartPosition"]
    joint_pos = cmdDict["jointPosition"]

    while len(joint_pos) < ROBOT_MAX_JOINT:
        joint_pos.append(0)

    try:
        resflag, jnt1, jnt2, jnt3, jnt4, jnt5, jnt6, jnt7 = c.kine_inverse(
            cart_pos, joint_pos
        )
        s.poll()
        rs = s.joint_safety
        robot_joint_num = s.robot_joint_num
        joint_pos = [jnt1, jnt2, jnt3, jnt4, jnt5, jnt6, jnt7]
        for i in range(0, robot_joint_num):
            max_limit = float(rs[i]["posLimt"]["positive_limit"])
            min_limit = float(rs[i]["posLimt"]["negative_limit"])
            if joint_pos[i] > max_limit or joint_pos[i] < min_limit:
                ret_dict["jointPosition"] = [jnt1, jnt2, jnt3, jnt4, jnt5, jnt6, jnt7]
                ret_dict["errorCode"] = "-18"
                ret_dict["errorMsg"] = (
                    "Target result is out of joint {}'s position limit".format(i)
                )
                return ret_dict
        if resflag:
            ret_dict["errorCode"] = "-4"
            ret_dict["errorMsg"] = "call kine_inverse failed"
            tcp_logger().info(
                "[SDK] kineinverse failed: {}, {}".format(cart_pos, joint_pos)
            )
            return ret_dict
        else:
            ret_dict["jointPosition"] = [jnt1, jnt2, jnt3, jnt4, jnt5, jnt6, jnt7]
            return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict["errorCode"] = "1"
        ret_dict["errorMsg"] = "Exception:function call failed"
        return ret_dict


def set_payload(**cmdDict):
    ret_dict = {}
    try:
        mass = cmdDict["mass"]
        centroid = cmdDict["centroid"]
        # tcp._logger().log_info('mass: {}, centroid: {}'.format(mass, centroid))
        c.set_payload(mass, centroid)
        # tcp_logger().log_info('[SDK] set payload failed: {}, {}'.format(mass, centroid))
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict["errorCode"] = "1"
        ret_dict["errorMsg"] = "Exception:function call failed"
        return ret_dict


def get_payload(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        s.payload
        ret_dict["mass"] = s.payload[0]
        ret_dict["centroid"] = s.payload[1]
        # tcp._logger().log_info('payload: {}'.format(s.payload))
        return ret_dict
    except Exception:
        ret_dict["errorCode"] = "1"
        ret_dict["errorMsg"] = "Exception:function call failed"
        return ret_dict


def set_clsn_sensitivity(**cmdDict):
    ret_dict = {}
    intid = cmdDict["sensitivityVal"]
    try:
        if int(intid) < 0 or int(intid) > 5:
            ret_dict["errorCode"] = "2"
            ret_dict["errorMsg"] = "Exception: Invalid arguments"
            return ret_dict
        sensitivityVal = intid
        c.set_clsn_sensi(sensitivityVal)
        tcp_logger().info("[SDK] set collision sensitivity: {}".format(sensitivityVal))
        return ret_dict
    except Exception:
        ret_dict["errorCode"] = "1"
        ret_dict["errorMsg"] = "Exception:function call failed"
        return ret_dict


def get_clsn_sensitivity(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        sensitivityLevel = s.clsn_sensitivity
        ret_dict["sensitivityLevel"] = sensitivityLevel
        return ret_dict
    except Exception:
        ret_dict["errorCode"] = "1"
        ret_dict["errorMsg"] = "Exception:function call failed"
        return ret_dict


def clear_error(**cmdDict):
    c.exit_protect(0)
    c.set_grpc_errcode(int(0), int(0), str(""), int(0xFF), int(0))
    ret_dict = {}
    tcp_logger().info("[SDK] clear error")
    return ret_dict


def shutdown(**cmdDict):
    c.state(3)
    c.state(1)
    c.shut_down()
    # threading.Timer(2,timer_shutdown).start()
    ret_dict = {}
    tcp_logger().info("[SDK] shut down the system")
    return ret_dict


def quit(**cmdDict):
    ret_dict = {}
    return ret_dict


def get_robot_state(**cmdDict):
    ret_dict = {}
    s.poll()
    if s.enabled == 1:
        ret_dict["enable"] = "robot_enabled"
    elif s.enabled == 0:
        ret_dict["enable"] = "robot_disabled"

    if s.powered_on == 1:
        ret_dict["power"] = "powered_on"
    elif s.powered_on == 0:
        ret_dict["power"] = "powered_off"

    return ret_dict


class ServopData:

    def __init__(self):
        self.select_alive = multiprocessing.Value("i", 0)
        self.servo_que = multiprocessing.Manager().list()
        self.servo_len = multiprocessing.Value("i", 0)


servop_data = ServopData()


def servo_move(**cmdDict):
    s.poll()
    enable = 0
    ret_dict = {}
    if cmdDict.has_key("enable"):
        enable = int(cmdDict["enable"])
    elif cmdDict.has_key("relFlag"):
        enable = int(cmdDict["relFlag"])
    else:
        ret_dict["errorCode"] = "1"
        ret_dict["errorMsg"] = "Exception:invalid parameters"
        return ret_dict
    try:
        if cmdDict.has_key("curoffFreq"):
            c.servojop_enable(enable, float(cmdDict["curoffFreq"]))
        else:
            c.servojop_enable(enable)

        t1 = time.time()
        while True:
            s.poll()
            if s.servo_move_enabled == enable:
                tcp_logger().log_info("[SDK] servo move enable: {}".format(enable))
                return ret_dict
            t2 = time.time()
            time.sleep(0.1)
            if t2 - t1 > 2:
                break
        tcp_logger().log_warn("[SDK] servo move enable timeout")
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def servo_select(servo_len, servo_que, select_alive):
    while servo_len.value and len(servo_que):
        data = servo_que[0]
        relFlag = int(data["relFlag"])
        stepNum = data["stepNum"]
        del servo_que[0]
        servo_len.value = len(servo_que)
        if data.has_key("catPosition"):
            cat_pos = data["catPosition"]
            c.servop(
                cat_pos[0],
                cat_pos[1],
                cat_pos[2],
                cat_pos[3],
                cat_pos[4],
                cat_pos[5],
                relFlag,
                stepNum,
            )
        elif data.has_key("jointPosition"):
            joint_pos = data["jointPosition"]

            while len(joint_pos) < ROBOT_MAX_JOINT:
                joint_pos.append(0)

            c.servoj(
                joint_pos[0],
                joint_pos[1],
                joint_pos[2],
                joint_pos[3],
                joint_pos[4],
                joint_pos[5],
                joint_pos[6],
                relFlag,
                stepNum,
            )

    select_alive.value = 0


def servo_j(**cmdDict):
    global servop_data
    s.poll()
    ret_dict = {}
    if s.motion_mode != 6:
        ret_dict["errorCode"] = "-1"
        ret_dict["errorMsg"] = "servoj command can only be excuted in servo move mode"
        return ret_dict

    joint_pos = cmdDict["jointPosition"]
    relFlag = int(cmdDict["relFlag"])
    stepNum = 1
    if cmdDict.has_key("stepNum"):
        stepNum = cmdDict["stepNum"]

    while len(joint_pos) < ROBOT_MAX_JOINT:
        joint_pos.append(0)
    c.servoj(
        joint_pos[0],
        joint_pos[1],
        joint_pos[2],
        joint_pos[3],
        joint_pos[4],
        joint_pos[5],
        joint_pos[6],
        relFlag,
        stepNum,
    )
    return ret_dict


def servo_p(**cmdDict):
    global servop_data

    s.poll()
    ret_dict = {}
    if s.motion_mode != 6:
        ret_dict["errorCode"] = "-1"
        ret_dict["errorMsg"] = "servop command can only be excuted in servo move mode"
        return ret_dict

    while servop_data.servo_len.value > 4:
        time.sleep(0.003)

    cat_pos = cmdDict["catPosition"]
    relFlag = cmdDict["relFlag"]
    stepNum = 1
    if cmdDict.has_key("stepNum"):
        stepNum = cmdDict["stepNum"]
    c.servop(
        cat_pos[0],
        cat_pos[1],
        cat_pos[2],
        cat_pos[3],
        cat_pos[4],
        cat_pos[5],
        relFlag,
        stepNum,
    )
    return ret_dict


def set_servo_move_filter(**cmdDict):
    s.poll()
    ret_dict = {}
    if s.motion_mode == 6:
        ret_dict["errorCode"] = "-1"
        ret_dict["errorMsg"] = (
            "servo move filter cannot be set when it is in servo move mode"
        )
        return ret_dict
    filter_type = cmdDict["filter_type"]
    if filter_type == 0:
        c.set_servoj_filter(0)
        tcp_logger().info("[SDK] servo move filter disabled")
        return ret_dict
    elif filter_type == 1:
        if cmdDict.has_key("lpf_cf"):
            lpf_cf = cmdDict["lpf_cf"]
            c.set_servoj_filter(filter_type, (lpf_cf,))
            tcp_logger().info(
                "[SDK] servo move filter: {}, {}".format(filter_type, lpf_cf)
            )
            return ret_dict
    elif filter_type == 2:
        if (
            cmdDict.has_key("nlf_max_vr")
            and cmdDict.has_key("nlf_max_ar")
            and cmdDict.has_key("nlf_max_jr")
        ):
            nlf_max_vr = cmdDict["nlf_max_vr"]
            nlf_max_ar = cmdDict["nlf_max_ar"]
            nlf_max_jr = cmdDict["nlf_max_jr"]
            c.set_servoj_filter(filter_type, (nlf_max_vr, nlf_max_ar, nlf_max_jr))
            tcp_logger().info(
                "[SDK] servo move filter: {}, {}".format(
                    filter_type, (nlf_max_vr, nlf_max_ar, nlf_max_jr)
                )
            )
            return ret_dict
    elif filter_type == 3:
        if (
            cmdDict.has_key("mmf_max_buf")
            and cmdDict.has_key("mmf_kp")
            and cmdDict.has_key("mmf_kv")
            and cmdDict.has_key("mmf_ka")
        ):
            mmf_max_buf = cmdDict["mmf_max_buf"]
            mmf_kp = cmdDict["mmf_kp"]
            mmf_kv = cmdDict["mmf_kv"]
            mmf_ka = cmdDict["mmf_ka"]
            c.set_servoj_filter(filter_type, (mmf_max_buf, mmf_kp, mmf_kv, mmf_ka))
            tcp_logger().info(
                "[SDK] servo move filter: {}, {}".format(
                    filter_type, (mmf_max_buf, mmf_kp, mmf_kv, mmf_ka)
                )
            )
            return ret_dict
    elif filter_type == 4:
        if (
            cmdDict.has_key("nlf_max_vp")
            and cmdDict.has_key("nlf_max_ap")
            and cmdDict.has_key("nlf_max_jp")
            and cmdDict.has_key("nlf_max_vr")
            and cmdDict.has_key("nlf_max_ar")
            and cmdDict.has_key("nlf_max_jr")
        ):
            nlf_max_vp = cmdDict["nlf_max_vp"]
            nlf_max_ap = cmdDict["nlf_max_ap"]
            nlf_max_jp = cmdDict["nlf_max_jp"]
            nlf_max_vr = cmdDict["nlf_max_vr"]
            nlf_max_ar = cmdDict["nlf_max_ar"]
            nlf_max_jr = cmdDict["nlf_max_jr"]
            c.set_servoj_filter(
                filter_type,
                (
                    nlf_max_vp,
                    nlf_max_ap,
                    nlf_max_jp,
                    nlf_max_vr,
                    nlf_max_ar,
                    nlf_max_jr,
                ),
            )
            tcp_logger().info(
                "[SDK] servo move filter: {}, {}".format(
                    filter_type,
                    (
                        nlf_max_vp,
                        nlf_max_ap,
                        nlf_max_jp,
                        nlf_max_vr,
                        nlf_max_ar,
                        nlf_max_jr,
                    ),
                )
            )
            return ret_dict
    elif filter_type == 5:
        if cmdDict.has_key("mmf_max_buf") and cmdDict.has_key("mmf_kp"):
            mmf_max_buf = cmdDict["mmf_max_buf"]
            mmf_kp = cmdDict["mmf_kp"]
            c.set_servoj_filter(filter_type, (mmf_max_buf, mmf_kp))
            tcp_logger().info(
                "[SDK] servo move filter: {}, {}".format(
                    filter_type, (mmf_max_buf, mmf_kp)
                )
            )
            return ret_dict
    else:
        ret_dict["errorCode"] = "-1"
        ret_dict["errorMsg"] = "unsupported filter type"
        tcp_logger().warn("[SDK] unsupported servo move filter {}".format(filter_type))
        return ret_dict

    ret_dict["errorCode"] = "-1"
    ret_dict["errorMsg"] = "required parameters missing for specified filter"
    return ret_dict


def is_in_servomove(**cmdDict):
    s.poll()
    ret_dict = {}
    ret_dict["in_servomove"] = s.motion_mode == 6
    return ret_dict


def torque_feedforward(**cmdDict):
    s.poll()
    if s.task_mode != jkzuc.MODE_AUTO:
        grvCurrent = cmdDict["grvCurrent"]
        includeGrvFlag = int(cmdDict["includeGrvFlag"])
        while len(grvCurrent) < ROBOT_MAX_JOINT:
            grvCurrent.append(0)
        c.current_feedforward(
            grvCurrent[0],
            grvCurrent[1],
            grvCurrent[2],
            grvCurrent[3],
            grvCurrent[4],
            grvCurrent[5],
            grvCurrent[6],
            includeGrvFlag,
        )
    ret_dict = {}
    return ret_dict


def get_data(**cmdDict):
    global delmitator
    s.poll()
    ret_dict = collections.OrderedDict()
    ret_dict["len"] = 0
    for k in maps_data:
        if k == "actual_position":
            ret_dict[k] = (getattr(s, "position"))[0:6]
        elif k == "joint_actual_position":
            ret_dict[k] = (getattr(s, "joint_position"))[0 : s.robot_joint_num]
        elif k == "tool_position":
            ret_dict[k] = (getattr(s, "tool_position"))[0:6]
        else:
            ret_dict[k] = getattr(s, k)
        ret_dict["drag_status"] = s.motion_mode == jkzuc.TRAJ_MODE_DRAG
    return ret_dict


def get_data_compress(**cmdDict):
    return get_data(**cmdDict)


def set_network_exception_handle(**cmdDict):
    ret_dict = {}
    try:
        timeLimit = float(cmdDict["timeLimit"]) / 1000
        motType = int(cmdDict["motType"])
        c.set_network_exception_handle(timeLimit, motType)

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_data_joint_position(**cmdDict):
    s.poll()
    tcp_logger().info("get_data_joint_position")
    ret_dict = {}
    ret_dict["joint_actual_position"] = (getattr(s, "joint_position"))[
        0 : s.robot_joint_num
    ]
    joint_actual_position = (getattr(s, "joint_position"))[0 : s.robot_joint_num]
    return joint_actual_position


def get_joint_position(**cmdDict):
    s.poll()
    tcp_logger().info("get_data_joint_position")
    ret_dict = {}
    ret_dict["joint_position"] = (getattr(s, "joint_position"))[0 : s.robot_joint_num]
    joint_actual_position = (getattr(s, "joint_position"))[0 : s.robot_joint_num]
    return ret_dict


def rapid_rate(**cmdDict):
    s.poll()
    rate_value = float(cmdDict["rate_value"])
    c.rapidrate(rate_value)
    ret_dict = {}
    tcp_logger().info("[SDK] set rapid rate {}".format(rate_value))
    return ret_dict


def torque_control_enable(**cmdDict):
    s.poll()
    enable_flag = int(cmdDict["enable_flag"])
    if enable_flag == 1:
        c.mode(4)
        c.wait_complete()
    else:
        c.mode(1)
        c.wait_complete()
    ret_dict = {}
    tcp_logger().info("[SDK] torque control enable {}".format(enable_flag))
    return ret_dict


def set_digital_output(**cmdDict):
    s.poll()
    io_type = cmdDict["type"]
    index = cmdDict["index"]
    value = cmdDict["value"]
    c.set_digital_output(io_type, index, value)
    tcp_logger().info("[SDK] set digital output {} {} {}".format(io_type, index, value))
    ret_dict = {}
    return ret_dict


def set_analog_output(**cmdDict):
    s.poll()
    io_type = cmdDict["type"]
    index = cmdDict["index"]
    value = cmdDict["value"]
    c.set_analog_output(io_type, index, value)
    ret_dict = {}
    tcp_logger().info("[SDK] set analog output {} {} {}".format(io_type, index, value))
    return ret_dict


def get_digital_input_status(**cmdDict):
    s.poll()
    ret_dict = {}
    ret_dict["din_status"] = s.din
    return ret_dict


def get_funcdi_status(**cmdDict):
    s.poll()
    ret_dict = {}
    ret_dict["funcdi_status"] = s.funcdi
    return ret_dict


def query_user_defined_variable(**cmdDict):
    try:
        ret_dict = {}
        var_list = []
        filename = jkzuc.jkroot_dir + "/configs/JAKA/jaka_user.var"
        with open(filename, "r+") as fd:
            for line in fd.readlines():
                line = line.rstrip()
                item_list = line.split()
                item_dict = {}
                item_dict["id"] = int(item_list[0])
                item_dict["value"] = float(item_list[1])
                item_dict["alias"] = str(item_list[2])
                var_list.append(item_dict)
        ret_dict["var_list"] = var_list
        return ret_dict
        # variable = user_variable.variable.add()
        # variable.id = int(item_list[0])
        # variable.value = float(item_list[1])
        # variable.alias = str(item_list[2])
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def drag_status(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict = {}
        ret_dict["drag_status"] = str((s.motion_mode == jkzuc.TRAJ_MODE_DRAG))
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def protective_stop_status(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["protective_stop"] = str(s.protective_stop)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def modify_user_defined_variable(**cmdDict):

    id_new = int(cmdDict["id_new"])
    alias_new = cmdDict["alias_new"]
    value_new = float(cmdDict["value_new"])

    try:
        ret_dict = {}
        line_list = []
        id_list = []
        alias_list = []
        c = jkzuc.command()
        # id_new = request.variable.id
        # alias_new = request.variable.alias
        # value_new = request.variable.value

        filename = jkzuc.jkroot_dir + "/configs/JAKA/jaka_user.var"
        with open(filename, "r+") as fd:
            line_list = fd.readlines()
            for line in line_list:
                line = line.rstrip()
                item_list = line.split()

                id_list.append(int(item_list[0]))
                alias_list.append(str(item_list[2]))
            tcp_logger().info("id_list: {}".format(id_list))
            tcp_logger().info("alias_list: {}".format(alias_list))

        for id in id_list:
            if id_new == id:
                id_index = id_list.index(id)
                alias_list.pop(id_index)
                for alias in alias_list:
                    if alias_new == alias:
                        ret_dict["errorCode"] = "3"
                        ret_dict["errorMsg"] = "The variable alias already exists"
                        return ret_dict

                line_list[id_index] = (
                    str(id_new) + "\t" + str(value_new) + "\t" + str(alias_new) + "\n"
                )
                break

        c.set_user_defined_variable(id_new, value_new)
        with open(filename, "w") as fd:
            fd.writelines(line_list)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def calib_dhparam(**cmdDict):
    try:
        ret_dict = {}
        # dhparams = cmdDict['dhparams']
        # dhparams_off = cmdDict['joint_homeoff']
        # c.calib_dhparam(dhparams[0],dhparams[1],dhparams[2],dhparams[3],dhparams[4],dhparams[5],dhparams_off)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_extio_status(**cmdDict):
    try:
        s.poll()
        extio = {}
        extio["num"] = s.extio_num
        extio["mode"] = s.extio_mode
        extio["setup"] = json.dumps(list(s.extio_setup))
        extio["pinmap"] = json.dumps(list(s.extio_pinmap))

        extio["status"] = json.dumps(list(s.extio_stat))
        ret_dict = {}

        ret_dict["extio_status"] = extio
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_user_offsets(**cmdDict):
    ret_dict = {}
    try:
        user_tooloffset = cmdDict["userffset"]
        user_id = cmdDict["id"]
        user_name = cmdDict["name"]
        user_method = cmdDict.get("method")
        if user_id == 0:
            ret_dict["errorCode"] = ErrCodeTcp.PARAM_ERROR.str_value
            ret_dict["errorMsg"] = "Illegal modification of robot base coordinates"
        if user_method is not None:
            c.set_muti_user_offsets(user_tooloffset, user_method, user_id, user_name)
        else:
            c.set_muti_user_offsets(user_tooloffset, 0, user_id, user_name)
        tcp_logger().info(
            "[SDK] set user offset {} {} {}".format(user_id, user_name, user_tooloffset)
        )
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_user_id(**cmdDict):
    ret_dict = {}
    try:
        user_frame_id = cmdDict["user_frame_id"]
        c.set_user_id(user_frame_id)
        tcp_logger().info("[SDK] set user id {}".format(user_frame_id))
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_curr_user_offset(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        user_offset = s.user_offset
        ret_dict["user_offset"] = user_offset
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_tool_offsets(**cmdDict):
    ret_dict = {}
    try:
        tooloff = cmdDict["tooloffset"]
        tool_name = cmdDict["name"]
        tool_id = cmdDict["id"]
        if tool_id == 0:
            ret_dict["errorCode"] = ErrCodeTcp.PARAM_ERROR.str_value
            ret_dict["errorMsg"] = "Illegal modification of robot base coordinates"
        tool_method = 0
        if cmdDict.has_key("method"):
            tool_method = cmdDict["method"]
        c.set_muti_tool_offsets(tooloff, tool_method, tool_id, tool_name)
        tcp_logger().info(
            "[SDK] set tool offset {} {} {}".format(tool_id, tool_name, tooloff)
        )

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_tool_id(**cmdDict):
    ret_dict = {}
    try:
        tool_id = cmdDict["tool_id"]
        c.set_tool_id(tool_id)
        tcp_logger().info("[SDK] set tool id {}".format(tool_id))
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_curr_tool_offset(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["tool_offset"] = s.tool_offset
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_tool_offsets(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["tool_offsets"] = s.tool_offsets
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_user_offsets(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["user_offsets"] = s.user_offsets
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_base_offset(**cmdDict):
    ret_dict = {}
    try:
        ret_dict["base_offset"] = s.installation_angle
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_dh_param(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["dh_param"] = s.dh_param
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_vrep_safe_zone(**cmdDict):
    ret_dict = {}
    s.poll()
    try:
        zone = s.vrep_safe_simulation_information
        spherelist = {}
        spherelist["elbow_center_offset"] = zone.get("elbow_center_offset")
        spherelist["elbow_sphere_radius"] = zone.get("elbow_sphere_radius")
        ret_dict["sphere_Param"] = spherelist
        toollist = {}
        tool_cone_dir = zone.get("tool_cone_dir")
        tool_dir_xyz = tool_cone_dir.get("tool_dir_xyz")
        toollist["a"] = tool_dir_xyz.get("a")
        toollist["b"] = tool_dir_xyz.get("b")
        toollist["c"] = tool_dir_xyz.get("c")
        ret_dict["tool_Param"] = toollist
        conelist = {}
        cone_xyz = tool_cone_dir.get("cone_xyz")
        conelist["a"] = cone_xyz.get("a")
        conelist["b"] = cone_xyz.get("b")
        conelist["c"] = cone_xyz.get("c")
        conelist["x_y"] = tool_cone_dir.get("x_y_cone")
        conelist["z"] = tool_cone_dir.get("z_cone")
        ret_dict["cone_Param"] = conelist
        id = 0
        planelist = []
        for plane in zone.get("planes"):
            planeparamlist = {}
            id = id + 1
            planeparamlist["planeEnable"] = plane.get("planeEnable")
            safezone = plane.get("safeZone")
            planeparamlist["x"] = safezone.get("x")
            planeparamlist["y"] = safezone.get("y")
            planeparamlist["z"] = safezone.get("z")
            planeparamlist["a"] = safezone.get("a")
            planeparamlist["b"] = safezone.get("b")
            planeparamlist["c"] = safezone.get("c")
            planelist.append(planeparamlist)
        ret_dict["plane_Param"] = planelist
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_installation_angle(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        if s.enabled == 1:
            ret_dict["errorCode"] = "1"
            ret_dict["errorMsg"] = "disable robot first"
            return ret_dict
        quat, appang = s.installation_angle
        appang_list = list(appang)
        appang_list[0] = cmdDict["angleX"]
        appang_list[1] = 0
        appang_list[2] = cmdDict["angleZ"]
        if not (0 <= appang_list[0] <= 180) or not (0 <= appang_list[2] <= 360):
            ret_dict["errorCode"] = "1"
            ret_dict["errorMsg"] = "exceed limit"
            return ret_dict
        appang = tuple(appang_list)
        ensure_mode(jkzuc.MODE_MANUAL)
        c.robot_set_base_offset(0, 0, 0, appang[0], appang[1], appang[2])
        tcp_logger().info("[SDK] set installation angle: {} {}".format(quat, appang))
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_installation_angle(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["installation_angle"] = s.installation_angle
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# jog(JOG_STOP,       jjogmode, ja_value)
# jog(JOG_CONTINUOUS, jjogmode, ja_value, speed)
# jog(JOG_INCREMENT,  jjogmode, ja_value, speed, increment)
def jog(**cmdDict):
    ret_dict = {}
    s.poll()
    try:
        jog_mode = cmdDict["jog_mode"]
        coord_map = cmdDict["coord_map"]
        ensure_mode(jkzuc.MODE_MANUAL)
        if coord_map == 0 and jog_mode != 0:
            for jnum in range(0, s.robot_joint_num):
                if not (s.homed[jnum]):
                    c.teleop_enable(0)
                    c.wait_complete()
                    c.home(-1)
                    c.wait_complete()
                    break
            c.teleop_enable(1)
            c.wait_complete
        elif coord_map == 1 and jog_mode != 0:
            c.teleop_enable(0)
            c.wait_complete()
        elif coord_map == 2 and jog_mode != 0:
            for jnum in range(0, s.robot_joint_num):
                if not (s.homed[jnum]):
                    c.teleop_enable(0)
                    c.wait_complete()
                    c.home(-1)
                    c.wait_complete()
                    break
            c.teleop_tool_enable(1)
            c.wait_complete()
        if jog_mode == 0:
            # JOG_STOP
            jnum = cmdDict["jnum"]
            c.jog(jog_mode, coord_map, jnum)
            c.abort()
            return ret_dict
        if s.inpos:
            if jog_mode == 2:
                # JOG_INCREMENT
                tcp_logger().warn("jog_mode: {}".format(jog_mode))
                jnum = cmdDict["jnum"]
                jogvel = cmdDict["jogvel"]
                poscmd = cmdDict["poscmd"]
                c.jog(jog_mode, coord_map, jnum, jogvel, poscmd)
            elif jog_mode == 1:
                # jog_mode = 1  JOG_CONTINUOUS
                tcp_logger().warn("jog_mode: {}".format(jog_mode))
                jnum = cmdDict["jnum"]
                jogvel = cmdDict["jogvel"]
                c.jog(jog_mode, coord_map, jnum, jogvel)
            elif jog_mode == 3:
                # jog_mode = 3  JOG_ABS
                jnum = cmdDict["jnum"]
                jogvel = cmdDict["jogvel"]
                poscmd = cmdDict["poscmd"]
                c.jog(jog_mode, coord_map, jnum, jogvel, poscmd)
            return ret_dict
        else:
            ret_dict["errorCode"] = "2"
            ret_dict["errorMsg"] = "robot is moving"
            return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def jog_stop(**cmdDict):
    global JOG_FLAG
    global JOGCON_WARNING
    ret_dict = {}
    intid = cmdDict["option"]
    try:
        s.poll()
        if s.enabled:
            c.jog(jkzuc.JOG_STOP, 1, intid)
            c.abort()
        JOG_FLAG = False
        JOGCON_WARNING = False
        return ret_dict
    except Exception:
        tcp_logger().warn("Exception:function call failed")
        retDict = {"errorCode": "3", "errorMsg": "Exception:function call failed"}
        return retDict


def set_flange_payload(**cmdDict):
    ret_dict = {}
    mass = cmdDict["mass"]
    xcom = cmdDict["xcom"]
    ycom = cmdDict["ycom"]
    zcom = cmdDict["zcom"]
    com = [xcom, ycom, zcom]
    try:
        c.set_payload(mass, com)
        return ret_dict
    except Exception:
        tcp_logger().warn("Exception:function call failed")
        ret_dict = {"errorCode": "3", "errorMsg": "Exception:function call failed"}
        return ret_dict


def kine_forward(**cmdDict):
    ret_dict = {}
    try:
        joints = cmdDict["jointPosition"]
        while len(joints) < ROBOT_MAX_JOINT:
            joints.append(0)

        resflag, px, py, pz, pa, pb, pc = c.kine_forward(joints)
        if resflag:
            ret_dict = {
                "errorCode": "2",
                "errorMsg": "Failure to find kinematics forward",
            }
            return ret_dict
        ret_dict["cartPosition"] = [px, py, pz, pa, pb, pc]
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_current_line(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["current_line"] = s.executing_line
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def mdi_enable(**cmdDict):
    ret_dict = {}
    try:
        c.mode(jkzuc.MODE_MDI)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_version(**cmdDict):
    ret_dict = {}
    try:
        tool_function_ctx = tool_function.ModuleCtx(tcp_logger)
        robot_id, robot_name = tool_function.read_robot_info(tool_function_ctx)
        ret_dict["version"] = jkzuc.controller_version
        ret_dict["robot_id"] = robot_id
        ret_dict["robot_name"] = robot_name
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def mdi_disable(**cmdDict):
    ret_dict = {}
    try:
        c.mode(jkzuc.MODE_MANUAL)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def mdi_execute(**cmdDict):
    ret_dict = {}
    try:
        c.mdi(cmdDict["cmdLine"])
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_joint_pos(**cmdDict):
    s.poll()
    try:
        ret_dict = {}
        joint_pos = list((getattr(s, "joint_position"))[0 : s.robot_joint_num])
        for i in range(0, s.robot_joint_num):
            joint_pos[i] = float("%.6f" % joint_pos[i])
        ret_dict["joint_pos"] = joint_pos
        del joint_pos
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_tcp_pos(**cmdDict):
    s.poll()
    try:
        ret_dict = {}
        tcp_pos = list((getattr(s, "position"))[0:6])
        for i in range(0, 6):
            tcp_pos[i] = float("%.6f" % tcp_pos[i])
        ret_dict["tcp_pos"] = tcp_pos
        del tcp_pos
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# /**
# * @brief 设置轨迹复现配置参数
# */
def set_traj_config(**cmdDict):
    try:
        acc = cmdDict["acc"]
        vel = cmdDict["vel"]
        xyz_interval = cmdDict["xyz_interval"]
        rpy_interval = cmdDict["rpy_interval"]
        try:
            filename = jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini"

            config = MyConfigParser()
            config.add_section("INTERVAL")
            config.set("INTERVAL", "xyz_interval", str(xyz_interval))
            config.set("INTERVAL", "rpy_interval", str(rpy_interval))

            config.add_section("ACC")
            config.set("ACC", "acc", str(acc))

            config.add_section("VEL")
            config.set("VEL", "vel", str(vel))

            with open(filename, "w+") as fd:
                config.write(fd)

            ret_dict = {}
            return ret_dict
        except Exception:
            tcp_logger().warn(traceback.format_exc())
            ret_dict = {
                "errorCode": "1",
                "errorMsg": "Exception:write parameter failed",
            }
            return ret_dict

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# /**
# * @brief 获取轨迹复现配置参数
# */
def get_traj_config(**cmdDict):
    ret_dict = {}
    try:
        xyz_interval = 0.1
        rpy_interval = 0.1
        acc = 300
        vel = 1000

        filename = jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini"
        if os.path.exists(filename):
            config = MyConfigParser()
            config.read(jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini")

            xyz_interval = float(config.get("INTERVAL", "xyz_interval"))
            rpy_interval = float(config.get("INTERVAL", "rpy_interval"))
            acc = float(config.get("ACC", "acc"))
            vel = float(config.get("VEL", "vel"))
        else:
            config = MyConfigParser()
            config.add_section("INTERVAL")
            config.set("INTERVAL", "xyz_interval", str(0.1))
            config.set("INTERVAL", "rpy_interval", str(0.1))

            config.add_section("ACC")
            config.set("ACC", "acc", str(300))

            config.add_section("VEL")
            config.set("VEL", "vel", str(1000))

            with open(filename, "w+") as fd:
                config.write(fd)

        ret_dict["acc"] = acc
        ret_dict["vel"] = vel
        ret_dict["xyz_interval"] = xyz_interval
        ret_dict["rpy_interval"] = rpy_interval
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# /**
# * @brief 采集轨迹复现数据控制开关
# */
def set_traj_sample_mode(**cmdDict):
    ret_dict = {}
    try:
        dirpath = jkzuc.jkroot_dir + "/scripts/track"
        if not os.path.exists(dirpath):
            os.mkdir(dirpath)
            time.sleep(0.5)
        if not os.access(dirpath, os.W_OK):
            os.chmod(dirpath, stat.S_IWRITE)
            time.sleep(0.5)

        name = cmdDict["filename"]
        if cmdDict["filename"] != "null":
            c.set_traj_sample_data_filename(str(name))
        c.traj_sample(int(cmdDict["mode"]))

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# /**
# * @brief 采集轨迹复现数据状态查询
# */
def get_traj_sample_status(**cmdDict):
    ret_dict = {}
    try:
        res = c.get_traj_sample_status()
        ret_dict["sampleStatus"] = res
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# /**
# * @brief 查询控制器中已经存在的轨迹复现数据的文件名
# */
def get_exist_traj_file_name(**cmdDict):
    ret_dict = {}
    try:
        dirpath = jkzuc.jkroot_dir + "/scripts/track"
        ret_dict["trajTrackFileName"] = os.listdir(dirpath)

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# /**
# * @brief 重命名轨迹复现数据的文件名
# */
def rename_traj_file_name(**cmdDict):
    ret_dict = {}
    try:
        dirpath = jkzuc.jkroot_dir + "/scripts/track"
        src = os.path.join(dirpath, cmdDict["src"])
        dest = os.path.join(dirpath, cmdDict["dest"])

        if len(cmdDict["dest"]) > 100 or len(cmdDict["dest"]) == 0:
            tcp_logger().log_warn("dest path is not valid.")
            ret_dict = {
                "errorCode": "1",
                "errorMsg": "Exception:filename rename failed",
            }
            return ret_dict

        if os.path.exists(dest):
            ret_dict = {
                "errorCode": "1",
                "errorMsg": "Exception:filename rename failed",
            }
            tcp_logger().log_warn("dest path already exist.")
            return ret_dict

        for file in os.listdir(src):
            if os.path.isfile(os.path.join(src, file)) == True:
                newname = file.replace(cmdDict["src"], cmdDict["dest"])
                os.rename(os.path.join(src, file), os.path.join(src, newname))

        for file in os.listdir(dirpath):
            if file == cmdDict["src"]:
                newname = file.replace(cmdDict["src"], cmdDict["dest"])
                os.rename(src, dest)

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# /**
# * @brief 删除控制器中轨迹复现数据文件
# */
def remove_traj_file(**cmdDict):
    ret_dict = {}
    try:
        dirpath = jkzuc.jkroot_dir + "/scripts/track"
        dirpath = os.path.join(dirpath, cmdDict["fileName"])
        if not os.path.exists(dirpath):
            ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}

        shutil.rmtree(dirpath)
        time.sleep(0.1)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# /**
# * @brief  控制器中轨迹复现数据文件生成控制器执行脚本
# */
def generate_traj_exe_file(**cmdDict):
    ret_dict = {}
    try:
        dirpath = jkzuc.jkroot_dir + "/scripts/track"
        dirpath = str(os.path.join(dirpath, cmdDict["fileName"]))
        if not os.path.exists(dirpath):
            ret_dict = {"errorCode": "1", "errorMsg": "Exception:filename not exists"}

        configpath = str(os.path.join(dirpath, "subtrajconfig.ini"))
        if os.path.exists(configpath):
            os.remove(configpath)

        config = MyConfigParser()
        config.read(jkzuc.jkroot_dir + "/configs/JAKA/trajconfig.ini")
        xyz_interval = float(config.get("INTERVAL", "xyz_interval"))
        rpy_interval = float(config.get("INTERVAL", "rpy_interval"))
        acc = float(config.get("ACC", "acc"))
        vel = float(config.get("VEL", "vel"))

        com = [xyz_interval, rpy_interval, vel, acc]

        res = c.generate_ngc_file(str(cmdDict["fileName"]), com)
        if not res[0]:
            ret_dict = {"errorCode": "1", "errorMsg": "Exception:function call failed"}

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_torsenosr_brand(**cmdDict):
    ret_dict = {}
    try:
        sensor_brand = cmdDict["sensor_brand"]
        c.set_torqsensor_brand(sensor_brand)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_torsenosr_brand(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["sensor_brand"] = int(s.torque_sensor_brand[0])
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# * [in] intid :0 辨识完成
#               1 辨识未完成
#               2 辨识失败
def RecordTorqSensorIdentifyData(endJoints):
    ret_dict = {}
    try:
        global IDENTIFY_COMPLETE
        global MASS
        global CENTROID
        ensure_mode(jkzuc.MODE_AUTO)

        while len(endJoints) < ROBOT_MAX_JOINT:
            endJoints.append(0)

        c.jog_movj(
            0,
            endJoints[0],
            endJoints[1],
            endJoints[2],
            endJoints[3],
            endJoints[4],
            endJoints[5],
            endJoints[6],
            3,
            90,
        )  # 30,90
        s.poll()
        move_flag = s.inpos
        with open("/home/jakauser/tooldata.txt", "w") as file:
            while not move_flag:
                s.poll()
                move_flag = s.inpos
                value = list(s.actual_position[3:6] + s.torqsensor[1][3])
                string = map(lambda x: str(x), value)
                value_sequence = " ".join(list(string))
                time.sleep(0.015)
                file.writelines(value_sequence + "\n")

        res = c.get_payload_auto_identify_result()
        ensure_mode(jkzuc.MODE_MANUAL)
        if res:
            MASS = res[0]
            CENTROID = (res[1][0], res[1][1], res[1][2])
            IDENTIFY_COMPLETE = 0  # 0 辨识完成
            return True
        else:
            IDENTIFY_COMPLETE = 2  # 0 辨识失败
            return False
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def start_torq_sensor_payload_identify(**cmdDict):
    ret_dict = {}
    try:
        endJoints = cmdDict["jointPosition"]
        # res = c.get_payload_auto_identify_result(startJoints,endJoints)
        t = threading.Thread(target=RecordTorqSensorIdentifyData, args=(endJoints,))
        t.start()
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_torq_sensor_payload_identify_result(**cmdDict):
    ret_dict = {}
    global IDENTIFY_COMPLETE
    global MASS
    global CENTROID
    try:
        IDENTIFY_COMPLETE = 1
        ret_dict["mass"] = MASS
        ret_dict["centroid"] = CENTROID
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_torq_sensor_identify_staus(**cmdDict):
    ret_dict = {}
    global IDENTIFY_COMPLETE
    try:
        ret_dict["identify_status"] = IDENTIFY_COMPLETE
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# * [in] : intid: 1---->  use a torque sensor
#                 0---->  doesn't use a torque sensor
def set_torque_sensor_mode(**cmdDict):
    ret_dict = {}
    intid = cmdDict["sensor_mode"]
    try:
        c.set_torqsensor_mode(intid)
        if intid == 1:
            t1 = time.time()
            while True:
                s.poll()
                if s.torqsensor[1][0] == 1:
                    tcp_logger().log_info("[SDK] torque sensor open succeeded")
                    return ret_dict
                t2 = time.time()
                time.sleep(0.1)
                if t2 - t1 > 2:
                    break
            tcp_logger().log_warn("[SDK] torque sensor open timeout")
            ret_dict = {"errorCode": "2", "errorMsg": "torque sensor open timeout"}
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


#    [in]:
#         intid : 0 is for tool  frame, 1 is for world frame
def set_ft_ctrl_frame(**cmdDict):
    ret_dict = {}
    try:
        intid = cmdDict["ftFrame"]
        c.set_admittance_ctrl_frame(intid)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_ft_ctrl_frame(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ftFrame = s.admittance_ctrl_frame[0]
        ret_dict["ftFrame"] = ftFrame
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_compliant_type(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        if not s.inpos:
            ret_dict = {"errorCode": "2", "errorMsg": "robot is moving"}
            return ret_dict
        compliantType = cmdDict["compliance_type"]
        compliantEnable = cmdDict["sensor_compensation"]
        c.set_compliant_type_enable(compliantType, compliantEnable)
        s.poll()
        t1 = time.time()
        while True:
            s.poll()
            if s.compliant_type_enable[0] == compliantType:
                return ret_dict
            time.sleep(0.1)
            if t2 - t1 > 2:
                break
        ret_dict = {
            "errorCode": "2",
            "errorMsg": "call fucntion set_compliant_type time out",
        }
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_compliant_type(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        value = s.compliant_type_enable
        ret_dict["compliance_type"] = value[0]
        ret_dict["sensor_compensation"] = value[1]
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_admit_ctrl_config(**cmdDict):
    ret_dict = {}
    try:
        axis = cmdDict["axis"]  # 0 ~5
        opt = cmdDict["opt"]
        ftUser = cmdDict["ftUser"]
        ftConstant = cmdDict["ftConstant"]
        ftNnormalTrack = cmdDict["ftNnormalTrack"]
        ftReboundFK = cmdDict["ftReboundFK"]
        ftconfig = (opt, ftUser, ftReboundFK, ftConstant, ftNnormalTrack)
        c.set_admitCtrl_config(axis, ftconfig, 1)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_admit_ctrl_config(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        value = s.admit_ctrl_config
        admit_ctrl_config = []
        for i in range(0, 6):
            admitConfig = []
            admitConfig.append(value[i][0])
            admitConfig.append(value[i][1])
            admitConfig.append(value[i][2])
            admitConfig.append(value[i][3])
            admitConfig.append(value[i][4])
            admit_ctrl_config.append(admitConfig)
        ret_dict["admitConfig"] = admit_ctrl_config
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def enable_admittance_ctrl(**cmdDict):
    ret_dict = {}
    try:
        intid = cmdDict["enable_flag"]
        if intid == 0:
            c.admitop_enable(0)
        else:
            c.admitop_enable(1)
            s.poll()
            t1 = time.time()
            while True:
                s.poll()
                if s.admittance_enabled:
                    tcp_logger().log_info("[SDK] enable admittance ctrl succes")
                    return ret_dict
                time.sleep(0.1)
                if t2 - t1 > 2:
                    break
            tcp_logger().log_warn("[SDK] enable admittance ctrl timeout")
            ret_dict = {
                "errorCode": "2",
                "errorMsg": "call enable_admittance_ctrl timeout",
            }
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# 设置传感器末端负载
def set_tool_payload(**cmdDict):
    ret_dict = {}
    try:
        m = cmdDict["mass"]
        centroid = cmdDict["centroid"]
        p = (centroid[0], centroid[1], centroid[2])
        c.set_tool_payload(m, p)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_tool_payload(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        toolPayLoadValue = s.toolPayLoad
        value = toolPayLoadValue[0]
        xcom = toolPayLoadValue[1][0]
        ycom = toolPayLoadValue[1][1]
        zcom = toolPayLoadValue[1][2]
        ret_dict["mass"] = value
        ret_dict["centroid"] = (xcom, ycom, zcom)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_torque_sensor_comm(**cmdDict):
    ret_dict = {}
    try:
        ip_addr = cmdDict["ip_addr"]
        port = cmdDict["port"]
        if cmdDict["type"] == 0:
            commInfo = (ip_addr, port)
            c.set_torqsensor_comm(0, commInfo)
        elif cmdDict["type"] == 1:
            c.set_torqsensor_comm(1)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_torque_sensor_comm(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        torqSnsrConfig = s.torqsensor[0]
        ret_dict["type"] = torqSnsrConfig[0]
        if torqSnsrConfig[0] == 0:  # network
            ret_dict["ip_addr"] = torqSnsrConfig[1][0]
            ret_dict["port"] = torqSnsrConfig[1][1]

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_torque_sensor_filter(**cmdDict):
    ret_dict = {}

    checker = NumCheckerCreator.GEZeroChecker()
    validator = Validator(checker)

    torqueSensorFilter = cmdDict["torqueSensorFilter"]
    validator.Validate(torqueSensorFilter)

    c.set_torque_sensor_filter(torqueSensorFilter)

    return ret_dict


def get_torque_sensor_filter(**cmdDict):
    ret_dict = {}
    s.poll()
    torqueSensorFilter = s.torque_sensor_filter[0]
    ret_dict["torqueSensorFilter"] = torqueSensorFilter
    return ret_dict


def set_torque_sensor_soft_limit(**cmdDict):
    ret_dict = {}

    checker = NumCheckerCreator.GEZeroChecker()
    validator = Validator(checker)

    torqueSensorSoftLimit = cmdDict["torqueSensorSoftLimit"]
    Fx = torqueSensorSoftLimit["Fx"]
    Fy = torqueSensorSoftLimit["Fy"]
    Fz = torqueSensorSoftLimit["Fz"]
    Mx = torqueSensorSoftLimit["Mx"]
    My = torqueSensorSoftLimit["My"]
    Mz = torqueSensorSoftLimit["Mz"]

    validator.Validate(Fx)
    validator.Validate(Fy)
    validator.Validate(Fz)
    validator.Validate(Mx)
    validator.Validate(My)
    validator.Validate(Mz)

    c.set_torque_sensor_soft_limit(Fx, Fy, Fz, Mx, My, Mz)
    return ret_dict


def get_torque_sensor_soft_limit(**cmdDict):
    ret_dict = {}
    s.poll()
    value = s.torque_sensor_soft_limit
    torqueSensorSoftLimit = {}
    torqueSensorSoftLimit["Fx"] = value[0]
    torqueSensorSoftLimit["Fy"] = value[1]
    torqueSensorSoftLimit["Fz"] = value[2]
    torqueSensorSoftLimit["Mx"] = value[3]
    torqueSensorSoftLimit["My"] = value[4]
    torqueSensorSoftLimit["Mz"] = value[5]
    ret_dict["torqueSensorSoftLimit"] = torqueSensorSoftLimit
    return ret_dict


def disable_force_control(**cmdDict):
    ret_dict = {}
    try:
        c.disable_force_control(1)

        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict




def set_compliance_condition(**cmdDict):
    ret_dict = {}
    try:
        comp_ctrl = cmdDict["compliant_condition"]

        c.set_compliant_condition(
            comp_ctrl[0],
            comp_ctrl[1],
            comp_ctrl[2],
            comp_ctrl[3],
            comp_ctrl[4],
            comp_ctrl[5],
        )
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_tio_vout_param(**cmdDict):
    ret_dict = {}
    try:
        tio_vout_ena = cmdDict["tio_vout_ena"]
        tio_vout_vol = cmdDict["tio_vout_vol"]
        c.tio_set_vout_ena(tio_vout_ena)
        c.tio_set_vout_vol(tio_vout_vol)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_tio_vout_param(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        tio_vout_param = s.tio_vout_cfg
        ret_dict["tio_vout_ena"] = tio_vout_param[0]
        ret_dict["tio_vout_vol"] = tio_vout_param[1]
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_servo_param(**cmdDict):
    ret_dict = {}
    try:
        jointId = cmdDict["jointId"]
        paramId = cmdDict["paramId"]
        value = cmdDict["value"]
        c.set_servo_param(jointId, paramId, value)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def add_tio_rs_signal(**cmdDict):
    ret_dict = {}
    try:
        strId = cmdDict["tio_signal_name"]
        chnId = cmdDict["tio_signal_chnId"]
        sigType = cmdDict["tio_signal_sigType"]
        sigAddr = cmdDict["tio_signal_sigAddr"]
        frequency = cmdDict["frequency"]
        if frequency > 10:
            ret_dict = {"errorCode": "4", "errorMsg": "Exception:param err frequency"}
            return ret_dict
        tcp_logger().info(
            "[SDK] tio_add_rs_signal: name:{} ,chnid:{} ,sigtype:{} ,sigaddr:{}".format(
                strId, chnId, sigType, sigAddr
            )
        )
        c.tio_add_rs_signal(strId, chnId, sigType, sigAddr)
        tcp_logger().info(
            "[SDK] tio_update_rs_signal: name:{} ,frequency:{}".format(strId, frequency)
        )
        c.tio_update_rs_signal(strId, frequency)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def del_tio_rs_signal(**cmdDict):
    ret_dict = {}
    try:
        signal_name = cmdDict["tio_signal_name"]
        tcp_logger().info("[SDK] tio_del_rs_signal: name:{} ".format(signal_name))
        c.tio_del_rs_signal(signal_name)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def send_tio_rs_command(**cmdDict):
    ret_dict = {}
    try:
        intid = cmdDict["chn_id"]
        cmdBuf = cmdDict["cmdBuf"]
        tcp_logger().info(
            "[SDK] tio_send_rs_command: intid:{} ,cmdBuf:{}".format(intid, cmdBuf)
        )
        c.tio_send_rs_command(intid, cmdBuf)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_rs485_signal_list(**cmdDict):
    try:
        s.poll()
        tioRSSig = {}
        tioRSSig["num"] = len(s.tio_rs_signals)
        for i in range(0, len(s.tio_rs_signals)):
            tio_single_info = {}
            if s.tio_rs_signals[i][0] != "":
                tio_single_info["chnId"] = s.tio_rs_signals[i][1]
                tio_single_info["sigType"] = s.tio_rs_signals[i][2]
                tio_single_info["sigAddr"] = s.tio_rs_signals[i][3]
                tio_single_info["value"] = s.tio_rs_signals[i][4]
                tio_single_info["frequency"] = s.tio_rs_signals[i][5]
                tioRSSig[str(s.tio_rs_signals[i][0])] = tio_single_info
        return tioRSSig
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_tio_pin_mode(**cmdDict):
    ret_dict = {}
    try:
        pinType = cmdDict["pinType"]
        pinMode = cmdDict["pinMode"]
        s.poll()
        if pinType >= 0 and pinType <= 2:
            if (
                (
                    (pinType == 1 and s.tio_pin_cfg[1] == 0xFF)
                    or (pinType == 2 and s.tio_pin_cfg[2] == 1)
                )
                and s.torqsensor[0][0] == 1
                and s.torqsensor[1][0] == 1
            ):
                tcp_logger().info(
                    "[SDK] the sensor base on this channel is now running"
                )
                ret_dict = {
                    "errorCode": "2",
                    "errorMsg": "Exception:function call failed",
                }
                return ret_dict
            tcp_logger().info(
                "[SDK] tio_set_pin_mode: pinType:{} ,pinMode:{}".format(
                    pinType, pinMode
                )
            )
            c.tio_set_pin_mode(pinType, pinMode)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_tio_pin_mode(**cmdDict):
    ret_dict = {"pinMode": None}
    try:
        s.poll()
        tio_vout_param = s.tio_vout_cfg

        pintype = cmdDict.get("pinType")
        if pintype is None:
            ret_dict["pinMode"] = [s.tio_pin_cfg[i] for i in range(3)]
        elif pintype in range(3):
            ret_dict["pinMode"] = s.tio_pin_cfg[pintype]
        else:
            tcp_logger().info("[SDK] get_tio_pin_mode pinType is invalid.")
            ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_rs485_chn_comm(**cmdDict):
    ret_dict = {}
    try:
        chn_id = cmdDict["chn_id"]
        slaveId = cmdDict["slaveId"]
        baudrate = cmdDict["baudrate"]
        databit = cmdDict["databit"]
        stopbit = cmdDict["stopbit"]
        parity = cmdDict["parity"]
        s.poll()
        baudrate_map = {4800, 9600, 14400, 19200, 38400, 57600, 115200, 230400}
        if not (baudrate in baudrate_map):
            tcp_logger().warn("[SDK] invaild param baudrate")
            ret_dict = {"errorCode": "3", "errorMsg": "invaild param baudrate"}
            return ret_dict
        if not (databit in {7, 8}):  # range in protobuf
            tcp_logger().warn("[SDK] invaild param databit")
            ret_dict = {"errorCode": "3", "errorMsg": "invaild param databit"}
            return ret_dict
        if not (stopbit in {1, 2}):  # range in protobuf
            tcp_logger().warn("[SDK] invaild param stopbit")
            ret_dict = {"errorCode": "3", "errorMsg": "invaild param stopbit"}
            return ret_dict
        if not (parity in {78, 79, 69}):  # range in protobuf
            tcp_logger().warn("the sensor base on this channel is now running")
            ret_dict = {"errorCode": "3", "errorMsg": "invaild param parity"}
            return ret_dict
        if (
            s.tio_rschn_cfg[chn_id][0] == 2
            and s.torqsensor[0][0] == 1
            and s.torqsensor[1][0] == 1
        ):
            ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
            return ret_dict
        commInfo = (slaveId, baudrate, databit, stopbit, parity)
        tcp_logger().info(
            "[SDK] tio_set_pin_mode: chn_id:{} ,slaveId:{} ,baudrate:{} ,databit:{} ,stopbit:{} ,parity:{}".format(
                chn_id, slaveId, baudrate, databit, stopbit, parity
            )
        )
        c.tio_set_rs485_chn_comm(chn_id, commInfo)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_rs485_chn_comm(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        chn_id = cmdDict["chn_id"]
        rtuSetting = s.tio_rschn_cfg[chn_id][1]
        ret_dict["slaveId"] = rtuSetting[0]
        ret_dict["baudrate"] = rtuSetting[1]
        ret_dict["databit"] = rtuSetting[2]
        ret_dict["stopbit"] = rtuSetting[3]
        ret_dict["parity"] = rtuSetting[4]
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_rs485_chn_mode(**cmdDict):
    ret_dict = {}
    try:
        chnId = cmdDict["chnId"]
        chnMode = cmdDict["chnMode"]
        s.poll()
        if chnId < 0 or chnId > 1:
            tcp_logger().warn("[SDK] invaild param chnId")
            ret_dict = {"errorCode": "3", "errorMsg": "invaild param chnId"}
            return ret_dict
        if chnMode == 2:
            if (chnId == 0 and s.tio_rschn_cfg[1][0] == 2) or (
                chnId == 1 and s.tio_rschn_cfg[0][0] == 2
            ):
                tcp_logger().warn("the sensor base on this channel is now running")
                ret_dict = {
                    "errorCode": "2",
                    "errorMsg": "Exception:function call failed",
                }
                return ret_dict
        else:
            # TIO RS channel is used as torque sensor, and it's now occupied
            if (
                s.tio_rschn_cfg[chnId][0] == 2
                and s.torqsensor[0][0] == 1
                and s.torqsensor[1][0] == 1
            ):
                tcp_logger().warn("the sensor base on this channel is now running")
                ret_dict = {
                    "errorCode": "2",
                    "errorMsg": "Exception:function call failed",
                }
                return ret_dict
        tcp_logger().info(
            "[SDK] tio_set_rs485_chn_mode: chnId:{} ,chnMode:{}".format(chnId, chnMode)
        )
        c.tio_set_rs485_chn_mode(chnId, chnMode)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_rs485_chn_mode(**cmdDict):
    ret_dict = {}
    try:
        chnId = cmdDict["chnId"]
        s.poll()
        ret_dict["chnMode"] = s.tio_rschn_cfg[chnId][0]
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def start_identify_friction(**cmdDict):
    ret_dict = {}
    try:
        jointId = cmdDict["jointId"]
        c.start_identify_friction(jointId)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def stop_identify_friction(**cmdDict):
    ret_dict = {}
    try:
        jointId = cmdDict["jointId"]
        c.stop_identify_friction(jointId)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_identify_friction_start_pos(**cmdDict):
    ret_dict = {}
    try:
        jointId = cmdDict["jointId"]
        pos = cmdDict["start_pos"]
        while len(pos) < ROBOT_MAX_JOINT:
            pos.append(0)
        c.set_identify_friction_start_pos(
            jointId, pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
        )
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_identify_friction_end_pos(**cmdDict):
    ret_dict = {}
    try:
        jointId = cmdDict["jointId"]
        pos = cmdDict["end_pos"]
        while len(pos) < ROBOT_MAX_JOINT:
            pos.append(0)
        c.set_identify_friction_end_pos(
            jointId, pos[0], pos[1], pos[2], pos[3], pos[4], pos[5], pos[6]
        )
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_port10000_delay_ms(**cmdDict):
    ret_dict = {}
    try:
        value = cmdDict["port10000_delay_ms"]
        if int(value) < 20:
            ret_dict = {"errorCode": "2", "errorMsg": "invalid parameter"}
            return ret_dict
        filename = "/usr/etc/jkzuc/configs/JAKA/optionalInfoConfig.ini"
        config = MyConfigParser()
        config.read(filename)
        config.set("PORTCONFIG", "port10000_delay_ms", str(value))
        with open(filename, "w+") as fd:
            config.write(fd)
        global port10000_delay_ms
        port10000_delay_ms = int(value)
        return ret_dict
    except Exception:
        tcp_logger().warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_port10000_delay_ms(**cmdDict):
    ret_dict = {}
    try:
        filename = "/usr/etc/jkzuc/configs/JAKA/optionalInfoConfig.ini"
        config = MyConfigParser()
        config.read(filename)
        value = config.getint("PORTCONFIG", "port10000_delay_ms")
        ret_dict["port10000_delay_ms"] = value
        return ret_dict
    except Exception:
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def getOptionalInfoConfig(**cmdDict):
    ret_dict = {}
    try:
        filename = "/usr/etc/jkzuc/configs/JAKA/optionalInfoConfig.ini"
        section = cmdDict["section"]
        key = cmdDict["key"]
        config = MyConfigParser()
        config.read(filename)
        value = config.getint(section, key)
        ret_dict["value"] = value
        return ret_dict
    except Exception:
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def setOptionalInfoConfig(**cmdDict):
    ret_dict = {}
    try:
        filename = "/usr/etc/jkzuc/configs/JAKA/optionalInfoConfig.ini"
        section = cmdDict["section"]
        key = cmdDict["key"]
        value = int(cmdDict["value"])
        config = MyConfigParser()
        config.read(filename)

        if config.has_option(section, key):
            config.set(section, key, str(value))
            with open(filename, "w+") as fd:
                config.write(fd)

            global maps_attr_used
            global maps_stat_used
            global port10000_delay_ms
            if section == section_attr:
                if key in maps_attr_used and value == 0:
                    maps_attr_used.remove(key)
                elif key not in maps_attr_used and value == 1:
                    maps_attr_used.append(key)
            elif section == section_stat:
                if key in maps_stat_used and value == 0:
                    maps_stat_used.remove(key)
                elif key not in maps_stat_used and value == 1:
                    maps_stat_used.append(key)
            elif section == section_port and key == "port10000_delay_ms":
                port10000_delay_ms = value
        else:
            ret_dict = {"errorCode": "2", "errorMsg": "invalid section or key"}

        return ret_dict
    except Exception:
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_torque_control_enable(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        if not s.enabled:
            ret_dict = {"errorCode": "-7", "errorMsg": "robot is not powered on"}
            return ret_dict
        if not s.powered_on:
            ret_dict = {"errorCode": "-6", "errorMsg": "robot is not enabled"}
            return ret_dict
        if not s.inpos:
            ret_dict = {"errorCode": "-17", "errorMsg": "robot is moving"}
            return ret_dict

        enable = cmdDict["enable"]
        period = cmdDict["period"]
        c.set_torque_control_enable(enable, period)
        tcp_logger().log_info(
            "[SDK] set_torque_control_enable: {} {}".format(enable, period)
        )
        ret_dict = {"errorCode": "0"}
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_torque_control_value(**cmdDict):
    if not s.torque_control_enable:
        ret_dict = {"errorCode": "-16", "errorMsg": "torque control is not enabled"}
        return ret_dict
    ret_dict = {}
    try:
        torque = cmdDict["torque"]
        while len(torque) < ROBOT_MAX_JOINT:
            torque.append(0)
        ret = c.set_torque_control_value(
            torque[0], torque[1], torque[2], torque[3], torque[4], torque[5], torque[6]
        )
        ret_dict = {"errorCode": "0", "number": ret}
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_full_dh_flag(**cmdDict):
    ret_dict = {}
    try:
        flag = cmdDict["flag"]
        ret = c.set_full_dh_flag(flag)
        t1 = time.time()
        while True:
            s.poll()
            if s.full_dh_flag:
                tcp_logger().log_info("[SDK] set_full_dh_flag succeeded")
                return ret_dict
            t2 = time.time()
            time.sleep(1)
            if t2 - t1 > 20:
                break
        tcp_logger().log_info("[SDK] set_full_dh_flag: {} timeout".format(flag))
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_gravity_direction(**cmdDict):
    ret_dict = {}
    try:
        rpy = cmdDict["rpy"]
        ret = c.set_gravity_direction(rpy[0], rpy[1], rpy[2])
        ret_dict = {"errorCode": "0"}
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def get_gravity_direction(**cmdDict):
    ret_dict = {}
    try:
        s.poll()
        ret_dict["rpy"] = s.gravity_direction
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_7_dof_inv_inform(**cmdDict):
    ret_dict = {}
    try:
        inv_flag = cmdDict["flag"]
        arm_angle = cmdDict["arm_angle"]
        c.set_7_dof_inv_inform(inv_flag, arm_angle)
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


def set_7_dof_avoid_limit_param(**cmdDict):
    ret_dict = {}
    try:
        k = cmdDict["flag"]
        alpha = cmdDict["arm_angle"]
        c.set_7_dof_avoid_limit_param(k, alpha)
        return ret_dict
    except Exception:
        tcp_logger().log_warn(traceback.format_exc())
        ret_dict = {"errorCode": "2", "errorMsg": "Exception:function call failed"}
        return ret_dict


# def set_torsnenosr_brand(**cmdDict):
#     ret_dict = {}
#     try:
#         compliantType =    cmdDict['compliance_type']
#         compliantEnable =  cmdDict['sensor_compensation']
#         c.set_compliant_type_enable(compliantType,compliantEnable)
#         return ret_dict
# except Exception:
#     print(traceback.format_exc())
#     ret_dict = {'errorCode':'2','errorMsg':'Exception:function call failed'}
#     return ret_dict

############################### TCP CMD PROCESS ############################


def process_tcp_command(jsonStr):
    cmd_dict = {
        # general part
        "enable_robot": enable_robot,
        "disable_robot": disable_robot,
        "power_on": power_on,
        "power_off": power_off,
        "shutdown": shutdown,
        "quit": quit,
        "get_robot_state": get_robot_state,
        "get_dh_param": get_dh_param,
        "set_installation_angle": set_installation_angle,
        "get_installation_angle": get_installation_angle,
        "get_base_offset": get_base_offset,
        "set_network_exception_handle": set_network_exception_handle,
        "set_port10000_delay_ms": set_port10000_delay_ms,
        "get_port10000_delay_ms": get_port10000_delay_ms,
        "getOptionalInfoConfig": getOptionalInfoConfig,
        "setOptionalInfoConfig": setOptionalInfoConfig,
        "get_version": get_version,
        # program
        "load_program": load_program,
        "get_loaded_program": get_loaded_program,
        "play_program": play_program,
        "pause_program": pause_program,
        "resume_program": resume_program,
        "stop_program": stop_program,
        "get_program_state": get_program_state,
        "query_user_defined_variable": query_user_defined_variable,
        "modify_user_defined_variable": modify_user_defined_variable,
        "get_current_line": get_current_line,
        # move cmd relateld
        "joint_move": joint_move,
        "end_move": end_move,
        "jog": jog,
        "jog_stop": jog_stop,
        "get_user_offsets": get_user_offsets,
        "set_user_id": set_user_id,
        "set_user_offsets": set_user_offsets,
        "get_curr_user_offset": get_curr_user_offset,
        "set_tool_id": set_tool_id,
        "set_tool_offsets": set_tool_offsets,
        "get_curr_tool_offset": get_curr_tool_offset,
        "get_tool_offsets": get_tool_offsets,
        "moveL": moveL,
        "movc": movc,
        "kine_inverse": kine_inverse,
        "kine_forward": kine_forward,
        "set_flange_payload": set_flange_payload,
        "set_payload": set_payload,
        "get_payload": get_payload,
        "get_joint_position": get_joint_position,
        "get_joint_pos": get_joint_pos,
        "get_tcp_pos": get_tcp_pos,
        "rapid_rate": rapid_rate,
        # servo
        "servo_move": servo_move,
        "servo_j": servo_j,
        "servo_p": servo_p,
        "set_servo_move_filter": set_servo_move_filter,
        "is_in_servomove": is_in_servomove,
        # IO
        "set_digital_output": set_digital_output,
        "set_analog_output": set_analog_output,
        "get_digital_input_status": get_digital_input_status,
        "get_funcdi_status": get_funcdi_status,
        "get_extio_status": get_extio_status,
        # traj
        "set_traj_config": set_traj_config,
        "get_traj_config": get_traj_config,
        "set_traj_sample_mode": set_traj_sample_mode,
        "get_traj_sample_status": get_traj_sample_status,
        "get_exist_traj_file_name": get_exist_traj_file_name,
        "rename_traj_file_name": rename_traj_file_name,
        "remove_traj_file": remove_traj_file,
        "generate_traj_exe_file": generate_traj_exe_file,
        # torque sensor and force control
        "set_torsenosr_brand": set_torsenosr_brand,
        "get_torsenosr_brand": get_torsenosr_brand,
        "start_torq_sensor_payload_identify": start_torq_sensor_payload_identify,
        "get_torq_sensor_payload_identify_result": get_torq_sensor_payload_identify_result,
        "get_torq_sensor_identify_staus": get_torq_sensor_identify_staus,
        "set_torque_sensor_mode": set_torque_sensor_mode,
        "set_ft_ctrl_frame": set_ft_ctrl_frame,
        "get_ft_ctrl_frame": get_ft_ctrl_frame,
        "set_compliant_type": set_compliant_type,
        "get_compliant_type": get_compliant_type,
        "set_admit_ctrl_config": set_admit_ctrl_config,
        "get_admit_ctrl_config": get_admit_ctrl_config,
        "enable_admittance_ctrl": enable_admittance_ctrl,
        "get_tool_payload": get_tool_payload,
        "set_tool_payload": set_tool_payload,
        "set_torque_sensor_comm": set_torque_sensor_comm,
        "get_torque_sensor_comm": get_torque_sensor_comm,
        "set_torque_sensor_filter": set_torque_sensor_filter,
        "get_torque_sensor_filter": get_torque_sensor_filter,
        "set_torque_sensor_soft_limit": set_torque_sensor_soft_limit,
        "get_torque_sensor_soft_limit": get_torque_sensor_soft_limit,
        "disable_force_control": disable_force_control,
        "set_compliance_condition": set_compliance_condition,
        "start_identify_friction": start_identify_friction,
        "stop_identify_friction": stop_identify_friction,
        "set_identify_friction_start_pos": set_identify_friction_start_pos,
        "set_identify_friction_en_pos": set_identify_friction_end_pos,
        # drag and collision
        "torque_control_enable": torque_control_enable,
        "drag_status": drag_status,
        "set_clsn_sensitivity": set_clsn_sensitivity,
        "get_clsn_sensitivity": get_clsn_sensitivity,
        "protective_stop_status": protective_stop_status,
        "clear_error": clear_error,
        # TIO
        "set_tio_vout_param": set_tio_vout_param,
        "get_tio_vout_param": get_tio_vout_param,
        "add_tio_rs_signal": add_tio_rs_signal,
        "del_tio_rs_signal": del_tio_rs_signal,
        "get_rs485_signal_list": get_rs485_signal_list,
        "send_tio_rs_command": send_tio_rs_command,
        "set_tio_pin_mode": set_tio_pin_mode,
        "set_rs485_chn_comm": set_rs485_chn_comm,
        "set_rs485_chn_mode": set_rs485_chn_mode,
        "get_tio_pin_mode": get_tio_pin_mode,
        "get_rs485_chn_comm": get_rs485_chn_comm,
        "get_rs485_chn_mode": get_rs485_chn_mode,
        # private interfaces and obsolete
        "mdi_enable": mdi_enable,
        "mdi_disable": mdi_disable,
        "mdi_execute": mdi_execute,
        "calib_dhparam": calib_dhparam,
        "get_data": get_data,
        "get_data_compress": get_data_compress,
        "set_servo_param": set_servo_param,
        "torque_feedforward": torque_feedforward,
        "get_vrep_safe_zone": get_vrep_safe_zone,
        "start_identify_friction": start_identify_friction,
        "stop_identify_friction": stop_identify_friction,
        "set_identify_friction_start_pos": set_identify_friction_start_pos,
        "set_identify_friction_en_pos": set_identify_friction_end_pos,
        "set_torque_control_enable": set_torque_control_enable,
        "set_torque_control_value": set_torque_control_value,
        "set_full_dh_flag": set_full_dh_flag,
        "set_gravity_direction": set_gravity_direction,
        "get_gravity_direction": get_gravity_direction,
        "set_7_dof_inv_inform": set_7_dof_inv_inform,
        "set_7_dof_avoid_limit_param": set_7_dof_avoid_limit_param,
    }
    retDict = {}
    try:
        # jsonStr = '{"cmdName":"servo_j","jointPosition":[10,10,10,10,10,10],"relFlag":0}'
        cmdDict = {}
        # print ("jsonstr(type): ",jsonStr, type(jsonStr)#type is unicode)
        cmdDict = json.loads(jsonStr)
        # print ("cmdDict: ", cmdDict #unicode)
        cmd_name = cmdDict["cmdName"]
        # print ("cmd_name" , cmd_name)
        retDict = cmd_dict[cmd_name](**cmdDict)
        # print ("retDict: ",retDict, type(retDict))
        retDict["cmdName"] = cmd_name
        if "errorCode" not in retDict:
            retDict["errorCode"] = "0"
            retDict["errorMsg"] = ""

        retData = ""
        if retDict.has_key("len"):
            retData = json.dumps(retDict)
            while retDict["len"] != retData.__len__():
                retDict["len"] = retData.__len__()
                retData = json.dumps(retDict)
        else:
            retData = json.dumps(retDict)
        # tcp_logger().info('retdata: {}'.format(retData))
        if cmd_name == "get_data_compress":
            retData = zlib.compress(retData)

    except TcpCommandException as e:
        tcp_logger().warn("{}, json str: {}".format(traceback.format_exc(), jsonStr))
        retDict = e.RetDict
        retData = json.dumps(retDict)

    except Exception:
        tcp_logger().warn("{}, json str: {}".format(traceback.format_exc(), jsonStr))
        e = UnknownException()
        retDict = e.RetDict
        retData = json.dumps(retDict)

    return retData
